# print "Loading Noitom script!"
script_dir = sys.argv[0]

# Command channels
import threading
import time

from math import sin, cos, radians

import sys
from __builtin__ import *
from vrKey import *
from vrOSGWidget import *
from vrScenegraph import *
from vrAEBase import *

# front/back/left/right/up/oblique
# king_T=[
#	[-5000, 0, 1200],
#	[5000, 0, 1200],
#	[0, -5000, 1200],
#	[0, 5000, 1200],
#	[0,0,5000],
#	[5000, -5000, 1200]]
#
# king_R=[
#	[0,0,-180],
#	[0,0,0],
#	[0,0,-90],
#	[0,0,90],
#	[0,-90,0],
#	[0,0,-45]]
king_T = [[400.161346, 150.548584, -250.294312]]
king_R = [[0, 0, 0]]
king_index = 0

global_height_offset = 0

# states definition


class States():

    def __init__(self):
        global script_dir

        index = 0
        self.idle = index
        index += 1
        self.need_reset_hmd = index
        index += 1
        self.reset_hmd_done = index
        index += 1
        self.need_record_chair = index
        index += 1
        self.record_chair_done = index
        index += 1
        self.need_goto_king = index
        index += 1
        self.goto_king_done = index
        index += 1
        self.need_start_play = index
        index += 1
        self.start_play_done = index
        index += 1
        self.need_reset_play = index
        index += 1
        self.reset_play_done = index
        index += 1

        self.configfile = open(script_dir + 'states_machine.log', 'w')

    def log(self, content):
        self.configfile.write(content + "\n")
        self.configfile.flush()

    def __del__(self):
        self.configfile.close()

# chair_pos_x=px
# chair_pos_y=py
# chair_pos_z=pz
# chair_rot_x=rx
# chair_rot_y=ry
# chair_rot_z=rz

# define class that calls a render update every frame


class CameraUpdate(vrAEBase):

    def __init__(self, camera_node, hmd_node, cmd_node):
        vrAEBase.__init__(self)
        self.states = States()
        self.state = self.states.idle
        self.counter = 0
        self.camera_node = camera_node
        self.hmd_node = hmd_node
        self.cmd_node = cmd_node
        self.noitom_root = findNode('NoitomRoot')
        print '__init__ ', getNodeName(camera_node)
        print '__init__ ', getNodeName(hmd_node)
        print '__init__ ', getNodeName(cmd_node)

        self.addLoop()
        self.isLooping = True
        print '__init__ addLoop'

        self.isPrintInfo = False

        self.keyC = vrKey(Key_C)  # manual reset/clear
        self.keyC.connect(self.reset_hmd)

        self.keyK = vrKey(Key_K)
        self.keyK.connect(goto_king_global, self)

        self.keyS = vrKey(Key_S)
        self.keyS.connect(start_scene)

        self.keyO = vrKey(Key_O)
        self.keyO.connect(reset_scene, self)

        self.reset_once = True

    # def __del__(self):
    #     self.isPrintInfo = False
    #     self.subLoop()

    def log(self, content):
        self.states.log(content)

    def recEvent(self, state):
        vrAEBase.recEvent(self, state)
        # print 'recEvent = ', getNodeName(self.camera_node)
        # print 'recEvent = ', state

    def loop(self):
        offset_pos = self.hmd_node.getChild(0).getWorldTranslation()

        self.camera_node.setWorldTranslation(
            offset_pos[0], offset_pos[1], offset_pos[2])
        # self.dump_info(T)

        self.state_machine()

        if self.reset_once:
            self.reset_hmd()

    def reset_hmd(self):
        offset_rot = self.hmd_node.getChild(0).getWorldRotation()
        if (abs(offset_rot[0]) < 70.0 and abs(offset_rot[1]) < 70.0):
            resetOculusRiftOrientation()
            self.camera_node.setRotation(0, 0, offset_rot[2])
            self.reset_once = False
            print "reset hmd ok"
        else:
            # print "reset hmd hanging..."
            pass

    def record_chair(self):
        global script_dir
        T = self.hmd_node.getTranslation()
        R = self.hmd_node.getRotation()

        #cam_from = getFrom(-1)
        #cam_at = getAt(-1)
        #cam_up = getUp(-1)

        with open(script_dir + 'chair.ini', 'w') as configfile:
            configfile.write('chair_pos_x=' + str(T[0]) + "\n")
            configfile.write('chair_pos_y=' + str(T[1]) + "\n")
            configfile.write('chair_pos_z=' + str(T[2]) + "\n")
            configfile.write('chair_rot_x=' + str(R[0]) + "\n")
            configfile.write('chair_rot_y=' + str(R[1]) + "\n")
            configfile.write('chair_rot_z=' + str(R[2]) + "\n")

        print "record_chair command ...\n"

    def read_chair(self, T, R):
        global script_dir

        config = open(script_dir + 'chair.ini', 'r')
        content = config.readline()
        while content:
            lst = content.split('=')
            name, val = lst[0], lst[1]
            val = float(val[0:len(val) - 1])
            if name == 'chair_pos_x':
                T[0] = val
            elif name == 'chair_pos_y':
                T[1] = val
            elif name == 'chair_pos_z':
                T[2] = val
            elif name == 'chair_rot_x':
                R[0] = val
            elif name == 'chair_rot_y':
                R[1] = val
            elif name == 'chair_rot_z':
                R[2] = val
            else:
                pass
            content = config.readline()

    def goto_king(self, king_t, king_r):
        print "\n...      goto_king      ..."
        T = [0, 0, 0]
        R = [0, 0, 0]
        self.read_chair(T, R)

        # ignore (king and chair)'s roll and pitch rotation
        king_rx, king_ry, king_rz = 0, 0, king_r[2]
        chair_rx,  chair_ry, chair_rz = 0, 0, R[2]
        # ignore (king and chair)'s height
        king_x, king_y, king_z = king_t[0], king_t[1], king_t[2]
        chair_x, chair_y, chair_z = T[0], T[1], 0

        print "king: position=[{0}, {1}, {2}]  rotation=[{3}, {4}, {5}]".format(king_x, king_y, king_z, king_rx, king_ry, king_rz)
        print "chair: position=[{0}, {1}, {2}]  rotation=[{3}, {4}, {5}]".format(chair_x, chair_y, chair_z, chair_rx, chair_ry, chair_rz)

        # step 1: create a temp chair node
        chair_node = findNode('noitom_chair')
        if not chair_node.isValid():
            chair_node = createNode('Transform3D', 'noitom_chair')
            chair_node.makeTransform()
        chair_node.setTranslation(chair_x, chair_y, chair_z)
        chair_node.setRotation(chair_rx, chair_ry, chair_rz)

        # step 2: clear the last noitom_root orientation and position to zero.
        self.noitom_root.setRotation(0, 0, 0)
        self.noitom_root.setTranslation(0, 0, 0)
        print "current HMD: position={0}  rotation={1}".format(self.hmd_node.getWorldTranslation(), self.hmd_node.getWorldRotation())

        # make noitom_root to as chair's child node without any change of orientation and position.
        # {{{{{
        print "make noitom_root as chair's child ..."

        # step 3: calculate the noitom_root's orientation complement
        chair_matrix = chair_node.getWorldTransform()
        # compensation_matrix =
        # [cos(radians(compensation_rot)),sin(radians(compensation_rot)),0,0,
        # -sin(radians(compensation_rot)),cos(radians(compensation_rot)),0,0,
        # 0,0,1,0, 0,0,0,1]
        compensation_matrix = chair_matrix
        compensation_matrix[3] = compensation_matrix[7] = compensation_matrix[11] = 0
        # execute the compensation rotate action
        self.noitom_root.setTransformMatrix(compensation_matrix, True)
        compensation_rot = self.noitom_root.getWorldRotation()
        # backtrace the matrix muliplication side-effect
        self.noitom_root.setTransformMatrix([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1], False)
        # execute the correct compensation without any side-effect
        self.noitom_root.setRotation(compensation_rot[0], compensation_rot[1], compensation_rot[2])

        # step 4: attach noitom_root node to chair node, and fix noitom_root's position
        chair_node.addChild(self.noitom_root)
        self.noitom_root.setWorldTranslation(0, 0, 0)
        print "current HMD: position={0}  rotation={1}".format(self.hmd_node.getWorldTranslation(), self.hmd_node.getWorldRotation())

        print "!!!!!    Look Up!   At this point: the HMD should stay the same.  !!!!!\n"
        # }}}}}

        # step 5: rotate chair to king's orientation and position
        chair_node.setRotation(king_rx, king_ry, king_rz)
        chair_node.setTranslation(king_x, king_y, king_z)

        # Now, the noitom_root node is at the correct position and orientation

        # step 6: get current noitom_root node position and orientation
        noitom_root_pos = self.noitom_root.getWorldTranslation()
        noitom_root_rot = self.noitom_root.getWorldRotation()
        print "noitom_root final:", noitom_root_pos, noitom_root_rot

        # step 7: deattach noitom_root node from chair node, and then attach to VRED Root node
        getRootNode().addChild(self.noitom_root)

        # step 8: set noitom_root to correct position and orientation
        self.noitom_root.setTranslation(noitom_root_pos[0], noitom_root_pos[1], noitom_root_pos[2])
        self.noitom_root.setRotation(noitom_root_rot[0], noitom_root_rot[1], noitom_root_rot[2])
        print "After goto_king, the final HMD:", self.hmd_node.getWorldTranslation(), self.hmd_node.getWorldRotation()

        # step 9: clear temp resources
        # deleteNode(chair_node, True)

        # step 10: trigger reset_hmd
        self.reset_once = True

    def dump_info(self, T):
        if self.isPrintInfo:
            self.counter += 1
            if self.counter > 100:
                print 'Pos: [', T[0], T[1], T[2], ']'
                self.counter = 0

    def state_machine(self):
        global king_T, king_R, king_index
        node_T = self.cmd_node.getTranslation()
        val = (abs(node_T[0] / 1000.0)**2 + abs(node_T[1] / 1000.0)**2 + abs(node_T[2] / 1000.0)**2)**0.5
        # print "val: "+str(node_T)+" state: " + str(self.state)

        if self.state == self.states.idle:
            if val < 10.0:
                # print "idle -> idle"
                pass
            elif val > 140 and val < 160:
                self.state = self.states.need_reset_hmd
                self.log("idle -> need_reset_hmd")
            elif val > 240 and val < 260:
                self.state = self.states.need_record_chair
                self.log("idle -> need_record_chair")
            elif val > 340 and val < 360:
                self.state = self.states.need_goto_king
                self.log("idle -> need_goto_king")
            elif val > 540 and val < 560:
                self.state = self.states.need_start_play
                self.log("idle -> need_start_play")
            elif val > 440 and val < 460:
                self.state = self.states.need_reset_play
                self.log("idle -> need_reset_play")
            else:
                self.log("Mocap Server sent invalid data")
        elif self.state == self.states.need_reset_hmd:
            self.log("reset_hmd now")
            self.reset_once = True
            self.state = self.states.reset_hmd_done
            self.log("need_reset_hmd -> reset_hmd_done")
        elif self.state == self.states.reset_hmd_done:
            if val < 10.0:
                self.state = self.states.idle
                self.log("reset_hmd_done -> idle")
            else:
                pass
        elif self.state == self.states.need_record_chair:
            self.log("record_chair now")
            self.record_chair()
            self.state = self.states.record_chair_done
            self.log("need_record_chair -> record_chair_done")
        elif self.state == self.states.record_chair_done:
            if val < 10.0:
                self.state = self.states.idle
                self.log("record_chair_done -> idle")
            else:
                pass
        elif self.state == self.states.need_goto_king:
            self.log("goto_king right now...")
            if king_index >= len(king_T):
                king_index = 0
            self.goto_king(king_T[king_index], king_R[king_index])
            king_index += 1
            self.state = self.states.goto_king_done
            self.log("need_goto_king -> goto_king_done")
        elif self.state == self.states.goto_king_done:
            if val < 10.0:
                self.state = self.states.idle
                self.log("goto_king_done -> idle")
            else:
                pass
        elif self.state == self.states.need_start_play:
            self.log("start play...")
            start_scene()
            self.state = self.states.start_play_done
            self.log("need_start_play -> start_play_done")
        elif self.state == self.states.start_play_done:
            if val < 10.0:
                self.state = self.states.idle
                self.log("start_play_done -> idle")
            else:
                pass
        elif self.state == self.states.need_reset_play:
            self.log("reset play ...")
            reset_scene(self)
            self.state = self.states.reset_play_done
            self.log("need_reset_play -> reset_play_done")
        elif self.state == self.states.reset_play_done:
            if val < 10.0:
                self.state = self.states.idle
                self.log("reset_play_done -> idle")
            else:
                pass
        else:
            self.log("Invalid State")


def camera_data_callback(obj):
    print 'called once: ', getNodeName(obj.camera_node)


def goto_king_global(obj):
    global king_T, king_R
    obj.record_chair()
    obj.goto_king(king_T[0], king_R[0])


def start_scene():
    selectVariantSet('start', False)


def reset_scene(obj):
    goto_king_global(obj)
    selectVariantSet('reset', False)


camera_update_obj = 0


def trans_connect(beacon_node, hmd_node, cmd_node):
    global camera_update_obj
    camera_update_obj = CameraUpdate(beacon_node, hmd_node, cmd_node)
    camera_update_obj.connect(camera_data_callback, camera_update_obj)
    camera_update_obj.setActive(True)
    camera_update_obj.callAllConnected()
    print "camera_update_obj state: ", camera_update_obj.isActive()


def applyTrans():
    beacon_node = findNode('NoitomBeacon')
    hmd_node = findNode('noitom_HMD')
    cmd_node = findNode("noitom_cmd")

    trans_connect(beacon_node, hmd_node, cmd_node)
    print 'TransScript ok'


def startNoitomTracking(vrpn_config_file):
    global script_dir
    loadTrackingConfig(script_dir + vrpn_config_file)
    startTracking()
    setDisplayMode(VR_DISPLAY_OCULUS_RIFT)
    print 'startNoitomTracking ok'


applyTrans()

startNoitomTracking('vredConfig.xml')
